import OpenGL 
OpenGL.ERROR_ON_COPY = True 
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from OpenGL.arrays import vbo
from OpenGL.GL.shaders import *

from ctypes import *
import numpy
#import math
#import time, sys

global haveCamera
haveCamera = False
trackIRMatrix = [0]*16
trackIRMatrix = numpy.array(trackIRMatrix, numpy.float32)
program = None
coordBuffer = []

trackir = cdll.LoadLibrary("./Debug/tirWrapper.dll")
trackirSetupCamera = getattr(trackir,"tirSetupCamera")
trackirShutdownCamera = getattr(trackir,"tirShutdownCamera")
trackirIsCamerasInitialized = getattr(trackir,"tirIsCameraInitialized")
trackirStartCamera = getattr(trackir,"tirStartCamera")
trackirStopCamera = getattr(trackir,"tirStopCamera")
trackirIsCameraRunning = getattr(trackir,"tirIsCameraRunning")
trackirRecenter = getattr(trackir,"tirRecenter")
trackirGetMatrix = getattr(trackir,"tirGetMatrix")
trackirHandle = trackir._handle

def deg2rad(degrees):
    return degrees * math.pi / 180

def rad2deg(radians):
    return radians * 180 / math.pi

def InitGL(Width, Height):                # We call this right after our OpenGL window is created.
    if not glUseProgram:
        print 'Missing Shader Objects!'
        sys.exit(1)
 
    global program
    program = compileProgram(
        compileShader('''
                    #version 120

                    attribute vec3 position;
                    attribute vec4 aColor;

                    varying vec4 vColor;
                    void main()
                    {
                       gl_Position = gl_ModelViewProjectionMatrix * vec4(position, 1.0);
                       vColor = aColor;
                    }
         ''',GL_VERTEX_SHADER),
        compileShader('''
		    #version 120

                    varying vec4 vColor;
                    uniform sampler2D uSampler;
                    
                    void main()
                    {
                       gl_FragColor = vColor;
                    }
         ''',GL_FRAGMENT_SHADER),
	)
    
    glClearColor(0.0, 0.0, 0.0, 0.0)    # This Will Clear The Background Color To Black
    glClearDepth(1.0)                    # Enables Clearing Of The Depth Buffer

    glEnable(GL_DEPTH_TEST)
    glDepthFunc(GL_LEQUAL)
    
    glEnable(GL_BLEND)
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
    
    glShadeModel(GL_SMOOTH)                # Enables Smooth Color Shading

    program.position = glGetAttribLocation(program, "position")
    program.color = glGetAttribLocation(program, "aColor")
    
    initBuffers()
    ReSizeGLScene(1024, 768)

def crateSquareXZ(vData, offsetX, offsetY, offsetZ, color, sizeX, sizeZ):
    square = [# coords,    color
            offsetX-0.0,   offsetY, offsetZ-0.0,   color[0],color[1],color[2],color[3],
            offsetX-0.0,   offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY, offsetZ-0.0,   color[0],color[1],color[2],color[3],
            offsetX-0.0,   offsetY, offsetZ-0.0,   color[0],color[1],color[2],color[3]
            ]
    vData.extend(square)

def crateSquareXY(vData, offsetX, offsetY, offsetZ, color, sizeX, sizeY):
    square = [# coords,    color
            offsetX-0.0,   offsetY-0.0,   offsetZ, color[0],color[1],color[2],color[3],
            offsetX-0.0,   offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY+sizeY, offsetZ, color[0],color[1],color[2],color[3],
            offsetX+sizeX, offsetY-0.0,   offsetZ, color[0],color[1],color[2],color[3],
            offsetX-0.0,   offsetY-0.0,   offsetZ, color[0],color[1],color[2],color[3]
            ]
    vData.extend(square)
    
def crateSquareZY(vData, offsetX, offsetY, offsetZ, color, sizeZ, sizeY):
    square = [# coords,    color
            offsetX, offsetY-0.0,   offsetZ-0.0,   color[0],color[1],color[2],color[3],
            offsetX, offsetY+sizeY, offsetZ-0.0,   color[0],color[1],color[2],color[3],
            offsetX, offsetY+sizeY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX, offsetY+sizeY, offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX, offsetY-0.0,   offsetZ+sizeZ, color[0],color[1],color[2],color[3],
            offsetX, offsetY-0.0,   offsetZ-0.0,   color[0],color[1],color[2],color[3]
            ]
    vData.extend(square)
    
def initBuffers():
    vData = [# coords,    color
            ]
    # Creates the walls
    crateSquareXY(vData, -50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)
    crateSquareXY(vData, -50, 0,  50, [0.788,0.788,0.788,1.0], 100, 20)
    crateSquareZY(vData, -50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)
    crateSquareZY(vData,  50, 0, -50, [0.788,0.788,0.788,1.0], 100, 20)

    # Creates the posts
    crateSquareXY(vData,  20, 0, -20, [1.0,0.0,0.0,1.0], 5, 20)
    crateSquareZY(vData,  20, 0, -25, [0.0,0.0,1.0,1.0], 5, 20)
    crateSquareXY(vData, -25, 0, -20, [1.0,0.0,0.0,1.0], 5, 20)
    crateSquareZY(vData, -20, 0, -25, [0.0,0.0,1.0,1.0], 5, 20)
    crateSquareXY(vData, -25, 0,  20, [1.0,0.0,0.0,1.0], 5, 20)
    crateSquareZY(vData, -20, 0,  20, [0.0,0.0,1.0,1.0], 5, 20)
    crateSquareXY(vData,  20, 0,  20, [1.0,0.0,0.0,1.0], 5, 20)
    crateSquareZY(vData,  20, 0,  20, [0.0,0.0,1.0,1.0], 5, 20)

    # Creates the floor
    colorCode = False
    for i in range(-50, 50, 5):
        colorCode = not colorCode
        for j in range(-50, 50, 5):
            colorCode = not colorCode
            if colorCode == False:
                color = [1.0,1.0,1.0,1.0]
            else:
                color = [0.0,0.0,0.0,1.0]
            crateSquareXZ(vData, i, 0, j, color, 5,5)
    crateSquareXZ(vData, -50, 20, -50, [0.215,0.705,1.0,1.0], 100,100) # Creates the celing
    crateSquareXY(vData, -5, 0, -49.9, [0.301,0.164,0.074,1.0], 10,15) # Creates the door
    crateSquareZY(vData, -49.95, 4.5, -10.5, [0.301,0.164,0.074,1.0], 11,11) # Creates the left window outline
    crateSquareZY(vData, -49.9, 5, -10, [0.215,0.705,1.0,1.0], 10,10) # Creates the left window
    crateSquareZY(vData, 49.95, 4.5, -30.5, [0.301,0.164,0.074,1.0], 31,11) # Creates the right window outline
    crateSquareZY(vData, 49.9, 5, -30, [0.215,0.705,1.0,1.0], 30,10) # Creates the right window

    vData = vertexBuffer(vData, 7, [3,4])
    global coordBuffer
    coordBuffer = vData
    
class vertexBuffer():
    def __init__(self, coords, item_size, item_stride):
        self.buffer = glGenBuffers(1)
        self.itemLen = len(coords)
        self.itemSize = item_size
        self.itemCount = self.itemLen / self.itemSize
        self.stride = item_stride

        self.itemLenB = self.itemLen * 4
        self.itemSizeB = self.itemSize * 4
        self.strideB = map(lambda x: ctypes.c_void_p(x * 4), self.stride)       

        glBindBuffer(GL_ARRAY_BUFFER, self.buffer)
        glBufferData(GL_ARRAY_BUFFER, numpy.array(coords, numpy.float32), GL_STATIC_DRAW)
        
# The function called when our window is resized (which shouldn't happen if you enable fullscreen, below)
def ReSizeGLScene(Width, Height):
    if Height == 0:                        # Prevent A Divide By Zero If The Window Is Too Small 
        Height = 1
    glViewport(0, 0, Width, Height)        # Reset The Current Viewport And Perspective Transformation
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(45.0, float(Width)/float(Height), 0.1, 5000.0)
    glMatrixMode(GL_MODELVIEW)

def pointerToMatrixR(data):
    matrix = [0]*16
    matrix[0] = data[0]
    matrix[1] = data[1]
    matrix[2] = data[2]
    matrix[3] = data[3]
    matrix[4] = data[4]
    matrix[5] = data[5]
    matrix[6] = data[6]
    matrix[7] = data[7]
    matrix[8] = data[8]
    matrix[9] = data[9]
    matrix[10] = data[10]
    matrix[11] = data[11]
    matrix[12] = data[12]
    matrix[13] = data[13]
    matrix[14] = data[14]
    matrix[15] = data[15]
    return numpy.array(matrix, numpy.float32)

# The main drawing function. 
def DrawGLScene():
        global trackIRMatrix
        matrix = (c_float*16)()
        matrix = cast(matrix, POINTER(c_float))
        if haveCamera:
            # This is where we get the matrix from the dll
            # We then convert it from c to python
            trackirGetMatrix(matrix, 1)
            matrix = pointerToMatrixR(matrix)
            
            # problem with x,y,z data
            matrix[12] /= 25 * -1
            matrix[13] /= 25 * -1
            matrix[14] /= 25 * -1
            #matrix[12] *= -1
            #matrix[13] *= -1
            #matrix[14] *= -1            
        # If theres a identity matrix, ignore it so the display dosent go back to the center
        if matrix[0] >= 1 and matrix[1] == 0 and matrix[2] == 0 and matrix[3] == 0:
            pass
        else:
            trackIRMatrix = matrix
            
        # Clear The Screen And The Depth Buffer
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
	glLoadIdentity()                    # Reset The View

        glUseProgram(program)

        glPushMatrix()

        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()
        glMultMatrixf(trackIRMatrix)
        glTranslatef(0,-5,0)
        #glTranslatef(z,y,z)


       
        glEnableVertexAttribArray(program.position)
        glEnableVertexAttribArray(program.color)

        glBindBuffer(GL_ARRAY_BUFFER, coordBuffer.buffer)
        glVertexAttribPointer(program.position, coordBuffer.stride[0], GL_FLOAT, False, coordBuffer.itemSizeB, None)
        glVertexAttribPointer(program.color, coordBuffer.stride[1], GL_FLOAT, False, coordBuffer.itemSizeB, coordBuffer.strideB[0])
        glDrawArrays(GL_TRIANGLES, 0, coordBuffer.itemCount)
        
        glDisableVertexAttribArray(program.color)
        glDisableVertexAttribArray(program.position)

        glPopMatrix()
        glUseProgram(0)
	glutSwapBuffers()

def keyPressed(*args):
	if args[0] == '\x1b':
            closeProgram()
	elif args[0] == 'c':
	    print "Recentering"
	    trackirRecenter()

def closeProgram():
    trackirStopCamera()
    trackirShutdownCamera()
    sys.exit()
	    
def main():
	global window
	fullscreen = False
 	glutInit(sys.argv)
 	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH)

	#glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE)
        glutInitWindowSize(1024, 768)
        glutInitWindowPosition(0, 0)
        window = glutCreateWindow("TrackIR In Python")

        # Callback functions
	glutDisplayFunc(DrawGLScene)
	glutIdleFunc(DrawGLScene)
	glutReshapeFunc(ReSizeGLScene)
	glutKeyboardFunc(keyPressed)

        InitGL(1024,768)
	glUseProgram(program)
	glutMainLoop()
 
# Print message to console, and kick off the main to get it rolling.
 
if __name__ == "__main__":
    trackirSetupCamera()
    if trackirIsCamerasInitialized():
        haveCamera = trackirStartCamera(1)
    main()
